#include "Int_MPU6050.h"

#define MPU6050_ADDRESS 0xD0 // MPU6050的I2C从机地址

/**
 * 函    数：MPU6050等待事件
 * 参    数：同I2C_CheckEvent
 * 返 回 值：无
 */
void MPU6050_WaitEvent(I2C_TypeDef *I2Cx, uint32_t I2C_EVENT)
{
	uint32_t Timeout;
	Timeout = 10000;								   // 给定超时计数时间
	while (I2C_CheckEvent(I2Cx, I2C_EVENT) != SUCCESS) // 循环等待指定事件
	{
		Timeout--;		  // 等待时，计数值自减
		if (Timeout == 0) // 自减到0后，等待超时
		{
			/*超时的错误处理代码，可以添加到此处*/
			break; // 跳出等待，不等了
		}
	}
}

/**
 * 函    数：MPU6050写寄存器
 * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
 * 参    数：Data 要写入寄存器的数据，范围：0x00~0xFF
 * 返 回 值：无
 */
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	I2C_GenerateSTART(I2C1, ENABLE);					   // 硬件I2C生成起始条件
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT); // 等待EV5

	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter); // 硬件I2C发送从机地址，方向为发送
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);   // 等待EV6

	I2C_SendData(I2C1, RegAddress);								 // 硬件I2C发送寄存器地址
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING); // 等待EV8

	I2C_SendData(I2C1, Data);									// 硬件I2C发送数据
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED); // 等待EV8_2

	I2C_GenerateSTOP(I2C1, ENABLE); // 硬件I2C生成终止条件
}

/**
 * 函    数：MPU6050读寄存器
 * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
 * 返 回 值：读取寄存器的数据，范围：0x00~0xFF
 */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;

	I2C_GenerateSTART(I2C1, ENABLE);					   // 硬件I2C生成起始条件
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT); // 等待EV5

	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter); // 硬件I2C发送从机地址，方向为发送
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);   // 等待EV6

	I2C_SendData(I2C1, RegAddress);								// 硬件I2C发送寄存器地址
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED); // 等待EV8_2

	I2C_GenerateSTART(I2C1, ENABLE);					   // 硬件I2C生成重复起始条件
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT); // 等待EV5

	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Receiver); // 硬件I2C发送从机地址，方向为接收
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED);	// 等待EV6

	I2C_AcknowledgeConfig(I2C1, DISABLE); // 在接收最后一个字节之前提前将应答失能
	I2C_GenerateSTOP(I2C1, ENABLE);		  // 在接收最后一个字节之前提前申请停止条件

	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED); // 等待EV7
	Data = I2C_ReceiveData(I2C1);							 // 接收数据寄存器

	I2C_AcknowledgeConfig(I2C1, ENABLE); // 将应答恢复为使能，为了不影响后续可能产生的读取多字节操作

	return Data;
}

/**
 * 函    数：MPU6050初始化
 * 参    数：无
 * 返 回 值：无
 */
void MPU6050_Init(void)
{
	/*开启时钟*/
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);  // 开启I2C1的时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); // 开启GPIOB的时钟

	/*GPIO初始化*/
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure); // 将PB10和PB11引脚初始化为复用开漏输出

	/*I2C初始化*/
	I2C_InitTypeDef I2C_InitStructure;										  // 定义结构体变量
	I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;								  // 模式，选择为I2C模式
	I2C_InitStructure.I2C_ClockSpeed = 50000;								  // 时钟速度，选择为50KHz
	I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;						  // 时钟占空比，选择Tlow/Thigh = 2
	I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;								  // 应答，选择使能
	I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // 应答地址，选择7位，从机模式下才有效
	I2C_InitStructure.I2C_OwnAddress1 = 0x00;								  // 自身地址，从机模式下才有效
	I2C_Init(I2C1, &I2C_InitStructure);										  // 将结构体变量交给I2C_Init，配置I2C1

	/*I2C使能*/
	I2C_Cmd(I2C1, ENABLE); // 使能I2C1，开始运行

	/*MPU6050寄存器初始化*/
	// MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);				//电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
	// MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);				//电源管理寄存器2，保持默认值0，所有轴均不待机
	// MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);				//采样率分频寄存器，配置采样率
	// MPU6050_WriteReg(MPU6050_CONFIG, 0x06);					//配置寄存器，配置DLPF
	// MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);			//陀螺仪配置寄存器，选择满量程为±2000°/s
	// MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);			//加速度计配置寄存器，选择满量程为±16g

	// 1. 复位MPU6050 复位->休眠200ms->唤醒
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 1 << 7);
	Delay_ms(200);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0);
	// 2. 设置量程
	// 2.1 角速度量程 +- 2000
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 3 << 3);
	// 2.2 加速度量程
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0);
	// 3. 设置其他参数
	// 关闭中断,关闭第2IIC接口,禁止FIFO
	MPU6050_WriteReg(MPU_INT_EN_REG, 0);
	MPU6050_WriteReg(MPU_USER_CTRL_REG, 0);
	MPU6050_WriteReg(MPU_FIFO_EN_REG, 0);
	// 4. 设置陀螺仪采样率和低通滤波器
	// 低通滤波器:数据输出速率时 1kHz
	MPU6050_WriteReg(MPU_CFG_REG, 1 << 0);
	// 采样率:采样率是多少的时候,才能正确的反应模拟信号
	// 采样率 = 陀螺仪输出频率 / (1 + SMPLRT_DIV)
	// 低通选择的是188,所以采样大于188*2即可
	// 我们可以设置为500
	// 500 = 1000 / (1 + x)
	MPU6050_WriteReg(MPU_SAMPLE_RATE_REG, 1);
	// 5. 时钟源
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 1);
	// 6. 使能角速度和加速度传感器(禁用待机模式)
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0);
}

/**
 * 函    数：MPU6050获取ID号
 * 参    数：无
 * 返 回 值：MPU6050的ID号
 */
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I); // 返回WHO_AM_I寄存器的值
}

/**
 * 函    数：MPU6050获取数据
 * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
 * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
 * 返 回 值：无
 */
void MPU6050_GetData(void)
{
	uint8_t DataH, DataL; // 定义数据高8位和低8位的变量

	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); // 读取加速度计X轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); // 读取加速度计X轴的低8位数据
	gyroAccel.accel.accelX = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); // 读取加速度计Y轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); // 读取加速度计Y轴的低8位数据
	gyroAccel.accel.accelY = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); // 读取加速度计Z轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); // 读取加速度计Z轴的低8位数据
	gyroAccel.accel.accelZ = (DataH << 8) | DataL; // 数据拼接，通过输出参数返回

	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); // 读取陀螺仪X轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); // 读取陀螺仪X轴的低8位数据
	gyroAccel.gyro.gyroX = (DataH << 8) | DataL;  // 数据拼接，通过输出参数返回

	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); // 读取陀螺仪Y轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); // 读取陀螺仪Y轴的低8位数据
	gyroAccel.gyro.gyroY = (DataH << 8) | DataL;  // 数据拼接，通过输出参数返回

	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); // 读取陀螺仪Z轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); // 读取陀螺仪Z轴的低8位数据
	gyroAccel.gyro.gyroZ = (DataH << 8) | DataL;  // 数据拼接，通过输出参数返回
}
